

#ifndef __MAP_PROCESS_H__
#define __MAP_PROCESS_H__

#include "map_data.h"
// #include "map_segment.h"
#include "PathPlan.h"

#include <geometry_msgs/Polygon.h>
#include <nav_msgs/OccupancyGrid.h>

#include <opencv2/opencv.hpp>

namespace global_path_planner {
namespace preprocess {

class MapSegment;

class MapProcess{
public:
    explicit MapProcess(std::shared_ptr<MapData>& map_data);
    virtual ~MapProcess();

    // 更新地图
    void UpdateMapImage(const nav_msgs::OccupancyGridConstPtr& grid_map,
                        std::shared_ptr<::common::planning::PathPlan>& path_plan_ptr,
                        const double bump_dis);

    // 预处理规划区域
    std::pair<int, cv::Mat> PreProcessPlanAreas(const cv::Mat& img, int& piexl_radius, bool& region_inflate,
                                                std::vector<geometry_msgs::Polygon>& all_polys);

    // 预处理功能区域
    std::vector<std::pair<int, cv::Mat>> PreProcessFuncAreas(cv::Mat& img);

    // 提取特征点
    std::unordered_map<int, std::vector<std::vector<cv::Point>>> GetAllContours(cv::Mat& img);

    // 提取规划区域内所有的特征点
    std::map<int, std::pair<cv::Mat, std::vector<std::vector<cv::Point>>>>
        getAllContours(cv::Mat &img, bool& loop);

    // 单个区域二值化图像输出
    cv::Mat MaskAreaImage(cv::Mat img, std::vector<std::vector<cv::Point>>& mask_area);

    // 规划区域内地图进行膨胀
    std::pair<int, cv::Mat> PlanAreaErode(int& piexl_radius, const cv::Mat& img);

    // 计算规划路径的轮廓
    std::unordered_map<int, std::vector<std::vector<cv::Point>>> ExtractPlanPathContours(std::vector<std::vector<std::pair<float, float>>>& paths, bool flag);

    // 虚拟墙或障碍物内部填充黑色表示不可达
    void FillNoConnectedArea(cv::Mat& map_grid);

    // 计算轮廓区域的面积
    float CalcPlanArea(std::vector<std::vector<cv::Point>>& polys);

    // 获取整张地图图像
    inline const cv::Mat& GetMapImg() const
    {
        return __map_Grid;
    }

private:

    // 处理虚拟墙
    void ProcessVirtueWalls(cv::Mat& map_grid);

    // 处理功能区
    std::vector<std::pair<int, std::vector<cv::Point>>> ProcessFuncAreas();

    // 处理规划外围区域和内部不可达区域
    std::vector<cv::Point> ProcessPolygons(geometry_msgs::Polygon& polygon);

    // 线段处理
    std::vector<cv::Point> LineProcess(std::vector<std::pair<float, float>>& line);

    // 圆形处理
    std::vector<cv::Point> CircleProcess(std::pair<std::pair<float, float>, float>& circle);

private:
    std::shared_ptr<MapData> __map_Data_Ptr;

private:
    // // 机器人半径
    // float __robot_Radius;
    // // 机器人扩展半径
    // float __extend_Radius;
    // 分辨率
    double __resolution;
    // 地图原点坐标
    double __origin_X;
    double __origin_Y;
    // 地图宽和高
    double __width;
    double __height;
    // 主刷长度
    float __brush_Length;

private:
    // v3_msgs::MapInfoConstPtr __map_Info_Ptr;
    cv::Mat __map_Grid;

};

}  // namespace preprocess
}  // namespace global_path_planner

#endif  // _MAP_PROCESS_H_